package crap;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.event.MouseAdapter;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.event.MouseMotionListener;
import java.awt.geom.AffineTransform;
import java.awt.geom.Ellipse2D;
import java.awt.geom.Path2D;
import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.awt.image.BufferedImage;
import java.util.ArrayList;

import javax.imageio.ImageIO;
import javax.swing.JComponent;
import javax.swing.JFrame;

import org.ejml.simple.SimpleMatrix;

public class CTester02 {
	public static void main(String[] args) throws Exception {
		final ArrayList<Point2D> markers = new ArrayList<Point2D>();
		final double mw = 320, mh = 240;
		final BufferedImage bi = ImageIO.read(CTester02.class.getClassLoader().getResourceAsStream("res/P5100210s.JPG"));
//		final BufferedImage bi = ImageIO.read(CTester02.class.getClassLoader().getResourceAsStream("res/P5100207s.JPG"));
		final Point2D[] camPoints = {new Point2D.Double(436-mw,166-mh), new Point2D.Double(613-mw,233-mh), new Point2D.Double(186-mw,281-mh), new Point2D.Double(61-mw, 197-mh)};
//		final Point2D[] camPoints = {new Point2D.Double(400-mw,160-mh), new Point2D.Double(464-mw,344-mh), new Point2D.Double(167-mw,393-mh), new Point2D.Double(119-mw, 191-mh)};
		final Point2D[] A4Points = {new Point2D.Double(0, 0), new Point2D.Double(0.210, 0), new Point2D.Double(0.210, 0.297), new Point2D.Double(0, 0.297)};
		final Path2D markerLine = new Path2D.Double();

		markerLine.moveTo(camPoints[0].getX()+mw, camPoints[0].getY()+mh);
		markerLine.lineTo(camPoints[1].getX()+mw, camPoints[1].getY()+mh);
		markerLine.lineTo(camPoints[2].getX()+mw, camPoints[2].getY()+mh);
		markerLine.lineTo(camPoints[3].getX()+mw, camPoints[3].getY()+mh);
		markerLine.closePath();
		
		JFrame ff = new JFrame();
		ff.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		
		final JComponent canvas = new JComponent() {
			{setPreferredSize(new Dimension(bi.getWidth(), bi.getHeight()));}
			@Override
			public void paint(Graphics g) {
				Graphics2D g2 = (Graphics2D)g;
				g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
				g2.setRenderingHint(RenderingHints.KEY_STROKE_CONTROL, RenderingHints.VALUE_STROKE_PURE);
				g2.setRenderingHint(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_QUALITY);
				
				g2.drawImage(bi, 0, 0, this);
				g2.setStroke(new BasicStroke(2f));
				g2.setColor(Color.red);
				g2.draw(markerLine);
				
				super.paint(g);
			}
		};
		
		final JFrame output = new JFrame();
		output.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);


		
		SimpleMatrix leftSide = new SimpleMatrix(8, 8), rightSide = new SimpleMatrix(8, 1);
		
		
		for (int i = 0; i<4; i++) {
			
			double x = A4Points[i].getX(), y = A4Points[i].getY(), x_ = camPoints[i].getX(), y_ = camPoints[i].getY();
			//x_ = (x y 1 0 0 0 -x_*x -x_*y ) h = x_
			leftSide.set(i*2, 0, x);
			leftSide.set(i*2, 1, y);
			leftSide.set(i*2, 2, 1);

			leftSide.set(i*2, 3, 0);
			leftSide.set(i*2, 4, 0);
			leftSide.set(i*2, 5, 0);
			
			leftSide.set(i*2, 6, -x_*x);
			leftSide.set(i*2, 7, -x_*y);
			
			//y_ = (0 0 0 x y 1 -y_*x -y_*y) h = y_
			leftSide.set(i*2+1, 0, 0);
			leftSide.set(i*2+1, 1, 0);
			leftSide.set(i*2+1, 2, 0);

			leftSide.set(i*2+1, 3, x);
			leftSide.set(i*2+1, 4, y);
			leftSide.set(i*2+1, 5, 1);
			
			leftSide.set(i*2+1, 6, -y_*x);
			leftSide.set(i*2+1, 7, -y_*y);
			
			// x_ und y_ im Ergebnisvektor setzen 
			rightSide.set(i*2, 0, x_);
			rightSide.set(i*2+1, 0, y_);
		}
		
		System.out.println(leftSide);
		System.out.println(rightSide);
		
//		System.out.println(leftSide.svd().getW());
//		System.out.println(leftSide.invert().mult(rightSide));
		SimpleMatrix h = leftSide.solve(rightSide);
		
		double h11 = h.get(0, 0), h12 = h.get(1,0), h13 = h.get(2, 0), h21 = h.get(3, 0), h22 = h.get(4, 0), h23 = h.get(5, 0), h31 = h.get(6, 0), h32 = h.get(7, 0);
		double lambda1 = Math.sqrt(h11*h11+h21*h21+h31*h31), lambda2 = Math.sqrt(h12*h12+h22*h22+h32*h32);
		
		System.out.println("lambda1: "+lambda1+"   lambda2: "+lambda2);
		
		double lambda = Math.max(lambda1, lambda2);
		double[][] hma = {{h11, h12, h13},{h21, h22, h23},{h31, h32, 1}};

		h11/=lambda;
		h12/=lambda;
		h13/=lambda;
		h21/=lambda;
		h22/=lambda;
		h23/=lambda;
		h31/=lambda;
		h32/=lambda;
		
		final double scale = 0.038*640/0.0173;
		
		double[][] rma = {{h11, h12, -(h21*h32-h22*h31)},{h21, h22, -(h31*h12-h32*h11)},{h31, h32, -(h11*h22-h21*h12)}};
		double[][] kma = {{scale, 0, 0},{0, scale, 0},{0, 0, 1}};
		
		SimpleMatrix H = new SimpleMatrix(hma);
		H.scale(1/lambda);
		
		SimpleMatrix K = new SimpleMatrix(kma);
		
		System.out.println("h11: "+h11+" h12: "+h12+" h13: "+h13+" h21: "+h21+" h22: "+h22+" h23: "+h23+" h31: "+h31+" h32: "+h32);
		
		SimpleMatrix result = new SimpleMatrix(4,4), R = new SimpleMatrix(rma), h3_ = new SimpleMatrix(3,1);
		
		Point2D oo = camPoints[2];
		SimpleMatrix p__ = new SimpleMatrix(new double[][]{{oo.getX()*scale}, {oo.getY()*scale}, {1}});
		
		final SimpleMatrix mapping = H.invert().mult(K.invert());
		
		h3_.set(0, 0, h13);
		h3_.set(1, 0, h23);
		h3_.set(2, 0, 1);
		
		System.out.println();
		System.out.println(R);
		System.out.println();
		System.out.println(h3_);
		System.out.println(R.invert().scale(-1).mult(h3_));
//		System.out.println(h);
		SimpleMatrix poopoo = mapping.mult(p__);
		
		poopoo = poopoo.scale(1/poopoo.get(2,0));
		System.out.println("poo: "+poopoo);
		final JComponent draw = new JComponent() {
			Rectangle2D A4 = new Rectangle2D.Double(0, 0, 0.21, 0.297);
			
			AffineTransform at = null;
			{setPreferredSize(new Dimension(500,300));
			at = AffineTransform.getScaleInstance(1000, 1000);
			at.preConcatenate(AffineTransform.getTranslateInstance(50, 10));
			}
			
			ArrayList<Point2D> toDraw = new ArrayList<Point2D>();
			
			Ellipse2D elli = new Ellipse2D.Double();
			@Override
			public void paint(Graphics g) {
				Graphics2D g2 = (Graphics2D)g;
				g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
				g2.setRenderingHint(RenderingHints.KEY_STROKE_CONTROL, RenderingHints.VALUE_STROKE_PURE);
				g2.setRenderingHint(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_QUALITY);
//				g2.transform(at);
				g2.draw(at.createTransformedShape(A4));
				toDraw.clear(); 
				toDraw.add(new Point2D.Double(0.21, 0.297));
				synchronized (markers) {
					
					for (final Point2D p : markers) {
						SimpleMatrix toTransform = new SimpleMatrix(new double[][] {{p.getX()*scale},{p.getY()*scale},{1}});
						SimpleMatrix neu = mapping.mult(toTransform);
						neu = neu.divide(neu.get(2, 0));
						toDraw.add(new Point2D.Double(neu.get(0, 0), neu.get(1, 0)));
					}
					
				}
				g2.setColor(Color.RED);
				for (Point2D p : toDraw) {
//					System.out.println(p);
					elli.setFrameFromCenter(p.getX(), p.getY(), p.getX()-0.001, p.getY()-0.001);
					g2.fill(at.createTransformedShape(elli));
				}
				super.paint(g);
			}
			
		};
		output.add(draw);
		output.pack();
		output.setBounds(640, 0, 500, 500);
		output.setVisible(true);
		
		MouseMotionListener ml = new MouseAdapter() {
			@Override
			public void mouseClicked(MouseEvent e) {
//				System.out.println(e.getPoint());
				synchronized (markers) {
					markers.add(new Point2D.Double(e.getX()-mw, e.getY()-mh));
					
				}
				super.mouseClicked(e);
				draw.repaint();
			}
			@Override
			public void mouseDragged(MouseEvent e) {
				synchronized (markers) {
					Point2D pu = new Point2D.Double(e.getX()-mw, e.getY()-mh);
//					System.out.println(pu);
					markers.add(pu);
					
				}
				super.mouseClicked(e);
				draw.repaint();
				super.mouseDragged(e);
			}
		};
		System.out.println(camPoints[2]);
		canvas.addMouseMotionListener(ml);
		ff.add(canvas);
		
		ff.pack();
		ff.setVisible(true);
		
	}
}
